/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;


import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.templates.commands.CommandBase;
import edu.wpi.first.wpilibj.templates.commands.ExampleCommand;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class RobotTemplate extends IterativeRobot {

    Command autonomousCommand;

    Victor l1,l2,r1,r2;
    
    Joystick left,right;
    
    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
        // instantiate the command used for the autonomous period
        autonomousCommand = new ExampleCommand();

        l1 = new Victor(1);
        l2 = new Victor(2);
        r1 = new Victor(4);
        r2 = new Victor(3);
        
        left = new Joystick(1);
        right = new Joystick(2);
        
        // Initialize all subsystems
        CommandBase.init();
    }

    public void autonomousInit() {
        // schedule the autonomous command (example)
        autonomousCommand.start();
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
        Scheduler.getInstance().run();
    }

    public void teleopInit() {
	// This makes sure that the autonomous stops running when
        // teleop starts running. If you want the autonomous to 
        // continue until interrupted by another command, remove
        // this line or comment it out.
        autonomousCommand.cancel();
        
        if(left.getRawButton(1) || right.getRawButton(1))
            //straight forward and backward
              if((Math.abs(left.getY())>0.1 && 
                      Math.abs(left.getX()) < 0.1) &&  
                              (Math.abs(right.getY())>0.1 && 
                                    Math.abs(right.getX())>0.1 )){
                    l1.set(-left.getY());//assume negative value in left go forward
                    l2.set(-left.getY());//assume positive value in right go forward
                    r1.set(right.getY());// check for sign of joystick magnitude first
                    r2.set(right.getY());  
              }
        //straight left and right
              if((Math.abs(left.getY())<0.1 && 
                      Math.abs(left.getX()) > 0.1) &&  
                              (Math.abs(right.getY())<0.1 && 
                                    Math.abs(right.getX())>0.1 )){
                  //assume the value that joystick goes left returns in x direction(horizontal) is negative, 
                  if(left.getX()<0 && right .getX() <0){
                        l1.set(left.getY());
                        l2.set(-left.getY());
                        r1.set(right.getY());
                        r2.set(-right.getY());
                  }
                  else if(left.getX()>0 && right .getX() >0){
                       l1.set(-left.getY());
                       l2.set(left.getY());
                       r1.set(-right.getY());
                       r2.set(right.getY());
                  }
                        
              }
              //diagonal
              if((Math.abs(left.getY())>0.1 && 
                      Math.abs(left.getX()) > 0.1) &&  
                              (Math.abs(right.getY())>0.1 && 
                                    Math.abs(right.getX())>0.1 )){
                                    //top left
                                    if((left.getX()<0 && right.getX()<0) &&
                                            (left.getY()<0 && right.getY() >0)){
                                        l2.set(-left.getY());
                                        r1.set(right.getY());
                                    }
                                    //top right
                                    if((left.getX()>0 && right.getX()>0) &&
                                            (left.getY()<0 && right.getY() >0)){
                                        l1.set(-left.getY());
                                        r2.set(right.getY());
                                    }
                                    //buttom left
                                    if((left.getX()<0 && right.getX()<0) &&
                                            (left.getY()>0 && right.getY() <0)){
                                        l1.set(left.getY());
                                        r2.set(-right.getY());
                                    }
                                    //buttom right
                                    if((left.getX()>0 && right.getX()>0) &&
                                            (left.getY()>0 && right.getY() <0)){
                                        l2.set(left.getY());
                                        r1.set(-right.getY());
                                    }
              }
        
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
        Scheduler.getInstance().run();
    }
    
    /**
     * This function is called periodically during test mode
     */
    public void testPeriodic() {
        LiveWindow.run();
    }
}
